﻿using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using AirSimUnity.DroneStructs;
using AirSimUnity.CarStructs;
using UnityEngine;

namespace AirSimUnity
{
    /*
     * An implementation of IAirSimInterface, facilitating calls from Unity to AirLib.
     * And also a bridge for the calls originating from AirLib into Unity.
     * Unity client components should use an instance of this class to interact with AirLib.
     *
     * IAirSimInterface的实现类，用于处理从Unity到AirLib的调用
     * 同时也是AirLib调用进入Unity的桥梁
     * Unity客户端组件应使用该类的实例与AirLib交互
     */

    internal class VehicleCompanion : IAirSimInterface
    {
        // 游戏中创建的所有载具
        private static readonly List<VehicleCompanion> Vehicles = new List<VehicleCompanion>();

        private static int basePortId;

        // 与Unity载具组件交互的接口
        private readonly IVehicleInterface VehicleInterface;

        private string vehicleName;
        private readonly bool isDrone;

        /// <summary>
        /// 静态构造函数，初始化委托函数
        /// </summary>
        static VehicleCompanion()
        {
            InitDelegators();
        }

        /// <summary>
        /// 私有构造函数，初始化载具伴侣对象
        /// </summary>
        /// <param name="vehicleInterface">载具接口</param>
        private VehicleCompanion(IVehicleInterface vehicleInterface)
        {
            VehicleInterface = vehicleInterface;
            isDrone = vehicleInterface is Drone;
            basePortId = AirSimSettings.GetSettings().GetPortIDForVehicle(isDrone);
        }

        /// <summary>
        /// 获取载具伴侣对象
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <param name="vehicleInterface">载具接口</param>
        /// <returns>载具伴侣对象</returns>
        public static VehicleCompanion GetVehicleCompanion(string vehicleName, IVehicleInterface vehicleInterface)
        {
            var companion = new VehicleCompanion(vehicleInterface)
            {
                vehicleName = vehicleName
            };

            Vehicles.Add(companion);

            Debug.Log("VehicleCompanion: " + vehicleName + Vehicles.Count);
            return companion;
        }

        /// <summary>
        /// 调用AirSim中的Tick方法
        /// </summary>
        /// <param name="deltaSecond">时间间隔</param>
        public void InvokeTickInAirSim(float deltaSecond)
        {
            PInvokeWrapper.CallTick(deltaSecond);
        }

        /// <summary>
        /// 调用AirSim中的碰撞检测方法
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <param name="collisionInfo">碰撞信息</param>
        public void InvokeCollisionDetectionInAirSim(string vehicleName, CollisionInfo collisionInfo)
        {
            PInvokeWrapper.InvokeCollisionDetection(vehicleName, collisionInfo);
        }

        /// <summary>
        /// 获取载具的运动状态
        /// </summary>
        /// <returns>运动状态</returns>
        public KinemticState GetKinematicState()
        {
            return PInvokeWrapper.GetKinematicState(vehicleName);
        }

        /// <summary>
        /// 获取录制数据
        /// </summary>
        /// <returns>图像数据</returns>
        public static DataRecorder.ImageData GetRecordingData()
        {
            return Vehicles[0].VehicleInterface.GetRecordingData();
        }

        /// <summary>
        /// 获取用于录制的相机捕获脚本
        /// </summary>
        /// <returns>相机捕获脚本</returns>
        public static DataCaptureScript GetCameraCaptureForRecording()
        {
            AirSimSettings.CamerasSettings recordCamSettings = AirSimSettings.GetSettings().Recording.Cameras[0];
            DataCaptureScript recordCam = Vehicles[0].VehicleInterface.GetCameraCapture(recordCamSettings.CameraName);
            return recordCam;
        }

        /// <summary>
        /// 注册委托函数到AirLib，基于IVehicleInterface
        /// </summary>
        private static void InitDelegators()
        {
            PInvokeWrapper.InitVehicleManager(
                Marshal.GetFunctionPointerForDelegate(new Func<AirSimPose, bool, string, bool>(SetPose)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, AirSimPose>(GetPose)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, CollisionInfo>(GetCollisionInfo)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, AirSimRCData>(GetRCData)),
                Marshal.GetFunctionPointerForDelegate(new Func<ImageRequest, string, ImageResponse>(GetSimImages)),
                Marshal.GetFunctionPointerForDelegate(new Func<int, RotorInfo, string, bool>(SetRotorSpeed)),
                Marshal.GetFunctionPointerForDelegate(new Func<bool, string, bool>(SetEnableApi)),
                Marshal.GetFunctionPointerForDelegate(new Func<CarControls, string, bool>(SetCarApiControls)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, CarState>(GetCarState)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, string, CameraInfo>(GetCameraInfo)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, AirSimPose, string, bool>(SetCameraPose)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, float, string, bool>(SetCameraFoV)),
                Marshal.GetFunctionPointerForDelegate(
                    new Func<string, string, float, string, bool>(SetDistortionParam)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, string, bool>(GetDistortionParams)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, int, bool, bool>(SetSegmentationObjectId)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, int>(GetSegmentationObjectId)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, string, string, int, bool>(PrintLogMessage)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, UnityTransform>(GetTransformFromUnity)),
                Marshal.GetFunctionPointerForDelegate(new Func<bool>(Reset)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, AirSimVector>(GetVelocity)),
                Marshal.GetFunctionPointerForDelegate(
                    new Func<AirSimVector, AirSimVector, string, RayCastHitResult>(GetRayCastHit)),
                Marshal.GetFunctionPointerForDelegate(new Func<float, bool>(Pause)),
                Marshal.GetFunctionPointerForDelegate(new Func<string, EnvironmentData>(GetEnvironmentData)) // 需要改为获取环境数据委托
            );
        }


        /*********************** Delegate functions to be registered with AirLib *****************************/

        private static EnvironmentData GetEnvironmentData(string vehicleName)
        {
            return new EnvironmentData()
            {
                data = "hello"
            };
        }

        /// <summary>
        /// 设置载具的姿态
        /// </summary>
        /// <param name="pose">姿态信息</param>
        /// <param name="ignoreCollision">是否忽略碰撞</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否设置成功</returns>
        private static bool SetPose(AirSimPose pose, bool ignoreCollision, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            vehicle.VehicleInterface.SetPose(pose, ignoreCollision);
            return true;
        }

        /// <summary>
        /// 获取载具的姿态
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>姿态信息</returns>
        private static AirSimPose GetPose(string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetPose();
        }

        /// <summary>
        /// 获取载具的碰撞信息
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>碰撞信息</returns>
        private static CollisionInfo GetCollisionInfo(string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetCollisionInfo();
        }

        /// <summary>
        /// 获取载具的遥控数据
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>遥控数据</returns>
        private static AirSimRCData GetRCData(string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetRCData();
        }

        /// <summary>
        /// 获取载具的模拟图像
        /// </summary>
        /// <param name="request">图像请求</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>图像响应</returns>
        private static ImageResponse GetSimImages(ImageRequest request, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetSimulationImages(request);
        }

        /// <summary>
        /// 从Unity获取载具的变换信息
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>变换信息</returns>
        private static UnityTransform GetTransformFromUnity(string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetTransform();
        }

        /// <summary>
        /// 重置所有载具
        /// </summary>
        /// <returns>是否重置成功</returns>
        private static bool Reset()
        {
            foreach (var vehicle in Vehicles)
            {
                vehicle.VehicleInterface.ResetVehicle();
            }

            return true;
        }

        /// <summary>
        /// 获取载具的速度
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>速度信息</returns>
        private static AirSimVector GetVelocity(string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetVelocity();
        }

        /// <summary>
        /// 获取射线投射的碰撞结果
        /// </summary>
        /// <param name="start">射线起点</param>
        /// <param name="end">射线终点</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>射线投射碰撞结果</returns>
        private static RayCastHitResult GetRayCastHit(AirSimVector start, AirSimVector end, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetRayCastHit(start, end);
        }

        /// <summary>
        /// 设置载具旋翼的速度
        /// </summary>
        /// <param name="rotorIndex">旋翼索引</param>
        /// <param name="rotorInfo">旋翼信息</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否设置成功</returns>
        private static bool SetRotorSpeed(int rotorIndex, RotorInfo rotorInfo, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.SetRotorSpeed(rotorIndex, rotorInfo);
        }

        /// <summary>
        /// 设置载具API是否启用
        /// </summary>
        /// <param name="enableApi">是否启用API</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否设置成功</returns>
        private static bool SetEnableApi(bool enableApi, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.SetEnableApi(enableApi);
        }

        /// <summary>
        /// 设置载具的控制参数
        /// </summary>
        /// <param name="controls">载具控制参数</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否设置成功</returns>
        private static bool SetCarApiControls(CarControls controls, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.SetCarControls(controls);
        }

        /// <summary>
        /// 获取载具的状态
        /// </summary>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>载具状态</returns>
        private static CarState GetCarState(string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetCarState();
        }

        /// <summary>
        /// 获取相机的信息
        /// </summary>
        /// <param name="cameraName">相机名称</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>相机信息</returns>
        private static CameraInfo GetCameraInfo(string cameraName, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetCameraInfo(cameraName);
        }

        /// <summary>
        /// 设置相机的姿态
        /// </summary>
        /// <param name="cameraName">相机名称</param>
        /// <param name="pose">姿态信息</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否设置成功</returns>
        private static bool SetCameraPose(string cameraName, AirSimPose pose, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.SetCameraPose(cameraName, pose);
        }

        /// <summary>
        /// 设置相机的视野角度
        /// </summary>
        /// <param name="cameraName">相机名称</param>
        /// <param name="fov_degrees">视野角度</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否设置成功</returns>
        private static bool SetCameraFoV(string cameraName, float fov_degrees, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.SetCameraFoV(cameraName, fov_degrees);
        }

        /// <summary>
        /// 设置相机的畸变参数
        /// </summary>
        /// <param name="cameraName">相机名称</param>
        /// <param name="paramName">参数名称</param>
        /// <param name="value">参数值</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否设置成功</returns>
        private static bool SetDistortionParam(string cameraName, string paramName, float value, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.SetDistortionParam(cameraName, paramName, value);
        }

        /// <summary>
        /// 获取相机的畸变参数
        /// </summary>
        /// <param name="cameraName">相机名称</param>
        /// <param name="vehicleName">载具名称</param>
        /// <returns>是否获取成功</returns>
        private static bool GetDistortionParams(string cameraName, string vehicleName)
        {
            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
            return vehicle.VehicleInterface.GetDistortionParams(cameraName);
        }

        /// <summary>
        /// 打印日志消息
        /// </summary>
        /// <param name="message">日志消息</param>
        /// <param name="messageParams">消息参数</param>
        /// <param name="vehicleName">载具名称</param>
        /// <param name="severity">日志级别</param>
        /// <returns>是否打印成功</returns>
        private static bool PrintLogMessage(string message, string messageParams, string vehicleName, int severity)
        {
            var vehicle = Vehicles[0]; // 只让第一个载具打印消息
            return vehicle.VehicleInterface.PrintLogMessage(message, messageParams, vehicleName, severity);
        }

        /// <summary>
        /// 设置分割对象的ID
        /// </summary>
        /// <param name="objectName">对象名称</param>
        /// <param name="objectId">对象ID</param>
        /// <param name="isNameRegex">名称是否为正则表达式</param>
        /// <returns>是否设置成功</returns>
        private static bool SetSegmentationObjectId(string objectName, int objectId, bool isNameRegex)
        {
            return Vehicle.SetSegmentationObjectId(objectName, objectId, isNameRegex);
        }

        /// <summary>
        /// 获取分割对象的ID
        /// </summary>
        /// <param name="objectName">对象名称</param>
        /// <returns>对象ID</returns>
        private static int GetSegmentationObjectId(string objectName)
        {
            return Vehicle.GetSegmentationObjectId(objectName);
        }

        /// <summary>
        /// 暂停所有载具
        /// </summary>
        /// <param name="timeScale">时间缩放比例</param>
        /// <returns>是否暂停成功</returns>
        private static bool Pause(float timeScale)
        {
            foreach (var vehicle in Vehicles)
            {
                vehicle.VehicleInterface.Pause(timeScale);
            }

            return true;
        }
    }
}